/*
  three_Servos
  2022-8-22

  本程序旨在使用Da-Servo库让4个舵机同时运动，通过串口发送A+度数进行控制。
    例如 A90

*/

#include "DaServo.h"

#define servoSpeed 10 //舵机运行速度 / Servo Speed

DaServo myservo1(0, servoSpeed); //建立舵机对象1
DaServo myservo2(1, servoSpeed); //建立舵机对象2
DaServo myservo3(2, servoSpeed); //建立舵机对象3
DaServo myservo4(3, servoSpeed); //建立舵机对象4

// 自定义IIC引脚
static const uint8_t mSDA = 8; // esp32默认为8
static const uint8_t mSCL = 6; // esp32默认为9

void setup()
{
    Serial.begin(9600);

    Wire.setPins(mSDA, mSCL);

    myservo1.init();
    myservo2.init();
    myservo3.init();
    myservo4.init();

    myservo1.setTarget(0); //初始设定舵机1运动目标位置 0 / myservo1 initial target
    myservo2.setTarget(0); //初始设定舵机2运动目标位置 0 / myservo2 initial target
    myservo3.setTarget(0); //初始设定舵机3运动目标位置 0 / myservo3 initial target
    myservo4.setTarget(0); //初始设定舵机4运动目标位置 0 / myservo4 initial target

    // 舵机回零
    while (myservo1.getPosition() != myservo1.getTarget())
    {
        myservo1.servoUpdate();
        myservo2.servoUpdate();
        myservo3.servoUpdate();
        myservo4.servoUpdate();
    }
}

void loop()
{
    myservo1.servoUpdate();
    myservo2.servoUpdate();
    myservo3.servoUpdate();
    myservo4.servoUpdate();
}
